//
// Created by fins on 24-10-25.
//
#include "lib/cube.hpp"
#include <iostream>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <chrono>

// 创造一个默认构造函数
Cube::Cube() : rotation(Eigen::Matrix3f::Identity()) {}

Cube::Cube(const Eigen::Vector3f& corner1, const Eigen::Vector3f& corner2)
        : rotation(Eigen::Matrix3f::Identity()) {

    // 计算中心和尺寸
    center = (corner1 + corner2) / 2.0f; // 计算中心
    size = (corner2 - corner1).cwiseAbs(); // 计算尺寸 cwiseAbs 求绝对值

    // 计算半尺寸
    float halfX = size.x() / 2.0f;
    float halfY = size.y() / 2.0f;
    float halfZ = size.z() / 2.0f;

    // 初始化每个面的方程参数
    planes[0][0] = 1;  planes[0][1] = 0;  planes[0][2] = 0;  planes[0][3] = -(center.x() + halfX); // +X面
    planes[1][0] = -1; planes[1][1] = 0;  planes[1][2] = 0;  planes[1][3] = center.x() - halfX; // -X面
    planes[2][0] = 0;  planes[2][1] = 1;  planes[2][2] = 0;  planes[2][3] = -(center.y() + halfY); // +Y面
    planes[3][0] = 0;  planes[3][1] = -1; planes[3][2] = 0;  planes[3][3] = center.y() - halfY; // -Y面
    planes[4][0] = 0;  planes[4][1] = 0;  planes[4][2] = 1;  planes[4][3] = -(center.z() + halfZ); // +Z面
    planes[5][0] = 0;  planes[5][1] = 0;  planes[5][2] = -1; planes[5][3] = center.z() - halfZ; // -Z面

    center2planes[0] = center.x() * planes[0][0] + center.y() * planes[0][1] + center.z() * planes[0][2] + planes[0][3];
    center2planes[1] = center.x() * planes[1][0] + center.y() * planes[1][1] + center.z() * planes[1][2] + planes[1][3];
    center2planes[2] = center.x() * planes[2][0] + center.y() * planes[2][1] + center.z() * planes[2][2] + planes[2][3];
    center2planes[3] = center.x() * planes[3][0] + center.y() * planes[3][1] + center.z() * planes[3][2] + planes[3][3];
    center2planes[4] = center.x() * planes[4][0] + center.y() * planes[4][1] + center.z() * planes[4][2] + planes[4][3];
    center2planes[5] = center.x() * planes[5][0] + center.y() * planes[5][1] + center.z() * planes[5][2] + planes[5][3];
    // 取出平面上的一个点
    center_planes[0] = Eigen::Vector3f(center.x() + halfX, center.y(), center.z());
    center_planes[1] = Eigen::Vector3f(center.x() - halfX, center.y(), center.z());
    center_planes[2] = Eigen::Vector3f(center.x(), center.y() + halfY, center.z());
    center_planes[3] = Eigen::Vector3f(center.x(), center.y() - halfY, center.z());
    center_planes[4] = Eigen::Vector3f(center.x(), center.y(), center.z() + halfZ);
    center_planes[5] = Eigen::Vector3f(center.x(), center.y(), center.z() - halfZ);
}

bool Cube::contains(const Eigen::Vector3f& point) const {
    // 检查点是否在所有六个面的内侧
    for (int i = 0; i < 6; ++i) {
        float A = planes[i][0];
        float B = planes[i][1];
        float C = planes[i][2];
        float D = planes[i][3];

        // 点代入平面方程
        float result1 = A * point.x() + B * point.y() + C * point.z() + D;
        float result = result1 * center2planes[i];
        if (result <= 0) { // 如果点在某个面的外侧
            return false;
        }
    }
    return true; // 如果点在所有面的内侧
}

void Cube::transform(const geometry_msgs::msg::TransformStamped& transform) {

    // 提取平移部分
    Eigen::Vector3f translation(transform.transform.translation.x,
                                transform.transform.translation.y,
                                transform.transform.translation.z);

    // 提取旋转部分并构建旋转矩阵
    tf2::Quaternion q(transform.transform.rotation.x,
                      transform.transform.rotation.y,
                      transform.transform.rotation.z,
                      transform.transform.rotation.w);
    tf2::Matrix3x3 m(q);

    // 声明三个 tf2Scalar 变量来存储 roll, pitch, yaw
    tf2Scalar roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);  // 获取 roll, pitch, yaw 角度

    // 如果你需要将这些角度转换为 Eigen 的旋转矩阵，可以使用以下代码
    Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ());

    // 首先计算旋转四元数
    Eigen::Quaternionf quaternion = yawAngle * pitchAngle * rollAngle;

    // 然后转换为旋转矩阵
    Eigen::Matrix3f rotationMatrix = quaternion.toRotationMatrix();

    // 旋转矩阵现在为 Eigen::Matrix3f rotationMatrix; 平移为 Eigen::Vector3f translation;

    for (int i = 0; i < 6; ++i) {
        float A = planes[i][0];
        float B = planes[i][1];
        float C = planes[i][2];
        float D = planes[i][3];
        Eigen::Vector3f normal(A, B, C);
        normal = rotationMatrix * normal;
        center_planes[i] = rotationMatrix * center_planes[i] + translation;
        planes[i][0] = normal.x();
        planes[i][1] = normal.y();
        planes[i][2] = normal.z();
        planes[i][3] =  -(normal.x() * center_planes[i].x() + normal.y() * center_planes[i].y() + normal.z() * center_planes[i].z());
    }


//    // 更新旋转矩阵
//    rotation = rotationMatrix * rotation; // 进行旋转
//
//    // 更新面的方程（重新计算D值）
//    for (int i = 0; i < 6; ++i) {
//        float A = planes[i][0];
//        float B = planes[i][1];
//        float C = planes[i][2];
//        float D = -(A * center.x() + B * center.y() + C * center.z()); // 重新计算D值
//        planes[i][3] = D;
//    }
//
//    // 重新计算A、B、C
//    for (int i = 0; i < 6; ++i) {
//        Eigen::Vector3f normal(planes[i][0], planes[i][1], planes[i][2]);
//        normal = rotation * normal;
//        planes[i][0] = normal.x();
//        planes[i][1] = normal.y();
//        planes[i][2] = normal.z();
//    }

    center = rotationMatrix  * center + translation; // 更新中心坐标
    center2planes[0] = center.x() * planes[0][0] + center.y() * planes[0][1] + center.z() * planes[0][2] + planes[0][3];
    center2planes[1] = center.x() * planes[1][0] + center.y() * planes[1][1] + center.z() * planes[1][2] + planes[1][3];
    center2planes[2] = center.x() * planes[2][0] + center.y() * planes[2][1] + center.z() * planes[2][2] + planes[2][3];
    center2planes[3] = center.x() * planes[3][0] + center.y() * planes[3][1] + center.z() * planes[3][2] + planes[3][3];
    center2planes[4] = center.x() * planes[4][0] + center.y() * planes[4][1] + center.z() * planes[4][2] + planes[4][3];
    center2planes[5] = center.x() * planes[5][0] + center.y() * planes[5][1] + center.z() * planes[5][2] + planes[5][3];

}

void Cube::print() const {
    std::cout << "Cube center: (" << center.x() << ", " << center.y() << ", " << center.z() << ")" << std::endl;
    std::cout << "Cube size: (" << size.x() << ", " << size.y() << ", " << size.z() << ")" << std::endl;
    std::cout << "Rotation matrix: " << std::endl << rotation << std::endl;
}
